#include<iostream>
#include<fstream>
#include<cstdlib>
#include "../LineExtraction.h"
#include "../LFOSCommon.h"

using namespace std;

int GetNumColors(ifstream& input);

typedef struct
{
	unsigned char R;
	unsigned char G;
	unsigned char B;
} Color;

int main(int argc, char* argv[])
{
	LineExtraction le;
	unsigned char inBuffer[PIXELS];
	unsigned char image[PIXELS];
	bool edgeImage[PIXELS];
	bool* edgePointer;
	unsigned char buffer[PIXELS];
	unsigned char outHeader[54];
	Color outImage[PIXELS];
	unsigned char* outBuffer;
	int nColors, size;
	ifstream inFile;
	ofstream outFile;
	Point edgePoints[PIXELS];
	int numPoints = 0;

	short cos[THETASIZE] = {35, 70, 105, 139, 174, 208, 242, 276, 309, 342, 375, 407, 438, 469, 500, 530, 559, 588,
			616, 643, 669, 695, 719, 743, 766, 788, 809, 829, 848, 866, 883, 899, 914, 927, 940, 951, 961, 970, 978, 985, 990, 995,
			998, 999, 1000, 999, 998, 995, 990, 985, 978, 970, 961, 951, 940, 927, 914, 899, 883, 866, 848, 829, 809, 788, 766, 743,
			719, 695, 669, 643, 616, 588, 559, 530, 500, 469, 438, 407, 375, 342, 309, 276, 242, 208, 174, 139, 105, 70, 35, 0};

	short sin[THETASIZE] = {-999, -998, -995, -990, -985, -978, -970, -961, -951, -940, -927, -914, -899, -883, -866,
			-848, -829, -809, -788, -766, -743, -719, -695, -669, -643, -616, -588, -559, -530, -500, -469, -438, -407, -375, -342, -309,
			-276, -242, -208, -174, -139, -105, -70, -35, 0, 35, 70, 105, 139, 174, 208, 242, 276, 309, 342, 375, 407, 438, 469, 500, 530,
			559, 588, 616, 643, 669, 695, 719, 743, 766, 788, 809, 829, 848, 866, 883, 899, 914, 927, 940, 951, 961, 970, 978, 985, 990,
			995, 998, 999, 1000};

//	short cos[THETASIZE] = {70, 139, 208, 276, 342, 407, 469, 530, 588, 643, 695, 743, 788, 829, 866, 899, 927, 951,
//		970, 985, 995, 999, 999, 995, 985, 970, 951, 927, 899, 866, 829, 788, 743, 695, 643, 588, 530, 469, 407, 342, 276, 208,
//		139, 70, 0};
//
//	short sin[THETASIZE] = {-998, -990, -978, -961, -940, -914, -883, -848, -809, -766, -719, -669, -616, -559, -500,
//		-438, -375, -309, -242, -174, -105, -35, 35, 105, 174, 242, 309, 375, 438, 500, 559, 616, 669, 719, 766, 809, 848, 883,
//		914, 940, 961, 978, 990, 998, 1000};

//	short cos[THETASIZE] = {87,174,259,342,423,500,574,643,707,766,819,866,906,940,966,985,996,1000,996,985,966,940,
//			906,866,819,766,707,643,574,500,423,342,259,174,87,0};
//
//	short sin[THETASIZE] = {-996,-985,-966,-940,-906,-866,-819,-766,-707,-643,-574,-500,-423,-342,-259,-174,-87,0,
//			87,174,259,342,423,500,574,643,707,766,819,866,906,940,966,985,996,1000};


	if(argc != 3)
	{
		cout << "Usage: LineExtractionTest <input file> <output file>\n";
		exit(1);
	}

//	inFile.open("edgeImage.bmp", ios::in | ios::binary);
	inFile.open(argv[1], ios::in | ios::binary);
	if(inFile.fail())
	{
		cout << "edgeImage.bmp does not exist\n";
		exit(1);
	}

//	outFile.open("lineImage.bmp", ios::out | ios::binary);
	outFile.open(argv[2], ios::out | ios::binary);
	if(outFile.fail())
	{
		cout << "Error opening lineImage.bmp\n";
		exit(1);
	}

	nColors = GetNumColors(inFile);

	inFile.seekg(54+4*nColors);
	//outFile.seekp(54+4*nColors);

	inFile.read((char*)inBuffer, PIXELS);


	//image stored upside down - need to invert rows
	for(int y=0; y < ROWS; y++)
		for(int x=0; x < COLS; x++)
		{
			image[ROW_COL_TO_INDEX(x,ROWS-y-1)] = inBuffer[ROW_COL_TO_INDEX(x,y)];
		}

    for(int i = 0; i < PIXELS; i++)
    {
    	if(image[i] == 255)
    	{
    		edgeImage[i] = true;
    		edgePoints[numPoints].x = INDEX_TO_COL(i);
    		edgePoints[numPoints++].y = INDEX_TO_ROW(i);
    	}
    	else
    		edgeImage[i] = false;
    }

    //create output bitmap header
    size = 54 + ROWS*(COLS*3 + ((COLS*3) % 4));
    //magic number
    outHeader[0] = 66;
    outHeader[1] = 77;
    //size in little endian
    outHeader[2] = size & 0xFF;
    outHeader[3] = (size >> 8) & 0xFF;
    outHeader[4] = (size >> 16) & 0xFF;
    outHeader[5] = (size >> 24) & 0xFF;
    //next four bytes unused
    outHeader[6] = 0;
    outHeader[7] = 0;
    outHeader[8] = 0;
    outHeader[9] = 0;
    //pixel array offset - 54 in little endian
    outHeader[10] = 54;
    outHeader[11] = 0;
    outHeader[12] = 0;
    outHeader[13] = 0;
    //number of bytes in DIB Header - 40 in little endian
    outHeader[14] = 40;
    outHeader[15] = 0;
    outHeader[16] = 0;
    outHeader[17] = 0;
    //width in little endian
    outHeader[18] = COLS & 0xFF;
    outHeader[19] = (COLS >> 8) & 0xFF;
    outHeader[20] = (COLS >> 16) & 0xFF;
    outHeader[21] = (COLS >> 24) & 0xFF;
    //Height in little endian
    outHeader[22] = ROWS & 0xFF;
    outHeader[23] = (ROWS >> 8) & 0xFF;
    outHeader[24] = (ROWS >> 16) & 0xFF;
    outHeader[25] = (ROWS >> 24) & 0xFF;
    //number of color planes - 1 in little endian
    outHeader[26] = 1;
    outHeader[27] = 0;
    //Number of bits per pixel - 24 in little endian
    outHeader[28] = 24;
    outHeader[29] = 0;
    //no Compression
    outHeader[30] = 0;
    outHeader[31] = 0;
    outHeader[32] = 0;
    outHeader[33] = 0;
    //size of image in little endian
    outHeader[34] = (size - 54) & 0xFF;
    outHeader[35] = ((size - 54) >> 8) & 0xFF;
    outHeader[36] = ((size - 54) >> 16) & 0xFF;
    outHeader[37] = ((size - 54) >> 24) & 0xFF;
    //horizontal resolution - 2835 pixels/meter
    outHeader[38] = 0;
    outHeader[39] = 0;
    outHeader[40] = 0;
    outHeader[41] = 0;
    //vertical resolution - 2835 pixels/meter
    outHeader[42] = 0;
    outHeader[43] = 0;
    outHeader[44] = 0;
    outHeader[45] = 0;
    //0 colors in palette
    outHeader[46] = 0;
    outHeader[47] = 0;
    outHeader[48] = 0;
    outHeader[49] = 0;
    //0 important colors
    outHeader[50] = 0;
    outHeader[51] = 0;
    outHeader[52] = 0;
    outHeader[53] = 0;

    outBuffer = new unsigned char[size-54];

    //initialize output image with edge detection
    Color pixel;
    for(int x=0; x < PIXELS; x++)
        {
        	if(edgeImage[x])
        	{
        		pixel.R = 255;
        		pixel.G = 255;
        		pixel.B = 255;
        	}
        	else
			{
				pixel.R = 0;
				pixel.G = 0;
				pixel.B = 0;
			}
        	outImage[x] = pixel;
        }

    LineSegment* segments;
    unsigned int numSegments;
    edgePointer = edgeImage;
    segments = le.ExtractLines(edgePointer, numSegments, edgePoints, numPoints);
    pixel.R = 255;
    pixel.G = 0;
    pixel.B = 0;

    cout << "Num segments: " << numSegments << endl;

    for(int i = 0; i < numSegments; i++)
    {
    	int theta, t;
    	int r;
    	int x, y;
    	Point p1;
    	Point p2;

    	theta = segments[i].theta;
    	r = segments[i].r;
    	p1 = segments[i].p1;
    	p2 = segments[i].p2;
    	cout << theta << "  " << r << "  (" << p1.x << "," << p1.y << ")  (" << p2.x << "," << p2.y << ")\n";

    	if(theta == 0)
    	{
    		for(y = p1.y; y <= p2.y; y++)
    			outImage[ROW_COL_TO_INDEX(r,y)] = pixel;
    	}
    	else if (theta >= -45 && theta <=45)
    	{
    		t = (theta - THETAMIN)/THETASTEP;
			//for(int y = 0; y < ROWS; y++)
    		for(int y = p1.y; y <= p2.y; y++)
			{
				//r = y*sin[t] + x*cos[t];
				x = r*TRIGFACTOR/cos[t] - y*sin[t]/cos[t];
				//y = x*cos[t]/sin[t] - r*TRIGFACTOR/sin[t];
				//y = (x*cos[t] - r)/sin[t]/TRIGFACTOR;
				outImage[ROW_COL_TO_INDEX(x,y)] = pixel;
			}
    	}
    	else
    	{
    		t = (theta - THETAMIN)/THETASTEP;
    		for(x = p1.x; x < p2.x; x++)
    		{
    			//r = y*sin[t] + x*cos[t];
    			y = r*TRIGFACTOR/sin[t] - x*cos[t]/sin[t];
    			//y = x*cos[t]/sin[t] - r*TRIGFACTOR/sin[t];
    			//y = (x*cos[t] - r)/sin[t]/TRIGFACTOR;
    			if(y >=0 && y <= ROWS -1)
    			{
    				outImage[ROW_COL_TO_INDEX(x,y)] = pixel;
    			}
    		}
    	}
    }

    //invert rows and place color in B G R order for output
	for(int y=0; y < ROWS; y++)
	{
		for(int x=0; x < COLS; x++)
		{
			int inIndex = ROW_COL_TO_INDEX(x,y);
			int outIndex = ROW_COL_TO_INDEX(x, ROWS-y-1)*3;
			outBuffer[outIndex] = outImage[inIndex].B;
			outBuffer[outIndex + 1] = outImage[inIndex].G;
			outBuffer[outIndex + 2] = outImage[inIndex].R;
		}
		//if row doesn't fall on 4 byte boundary - pad with zeros
		if(((COLS*3) % 4) != 0)
		{
			int index = ROW_COL_TO_INDEX(COLS, ROWS-y-1)*3;
			for(int i = 0; i < (COLS*3)%4; i++)
				outBuffer[index+i] = 0;
		}
	}

	//write data to file
	outFile.write((char*)outHeader, 54);
    outFile.write((char*)outBuffer, size-54);

	inFile.close();
	outFile.close();

	cout << "Line Extraction Test Complete" << endl;

	return 0;
}

int GetNumColors(ifstream& input)
{
	int nColors;
	unsigned char buffer[4];

	//read number of colors
	input.seekg(46);
	input.read((char*)buffer, 4);
	nColors = buffer[0] | (buffer[1] << 8) |
			(buffer[2] << 16) | (buffer[3] << 24);

	return nColors;
}
